#include <stdio.h>
#include <stdint.h>
#include <math.h>
#include "colorspace.h"

/* float RGB2Yxy = [ 0.5141364, 0.3238786, 0.16036376, */
/*                   0.265068, 0.67023428, 0.06409157, */
/*                   0.0241188, 0.1228178, 0.84442666 ]; */
/* float eps = 0.000001; */

/* float Yxy2RGB = [ 2.5651, -1.1665, -0.3986, */
/*                   -1.0217, 1.9777, 0.0439, */
/*                   0.0753, -0.2543, 1.1892 ]; */


//http://brucelindbloom.com/index.html?Eqn_RGB_XYZ_Matrix.html


//sRGB	D65
static float RGB2XYZ[3][3] = {
    {0.4124564,0.3575761,0.1804375},
    {0.2126729,0.7151522,0.0721750},
    {0.0193339,0.1191920,0.9503041},
};

static float XYZ2RGB[3][3] = {
    {3.2404542,-1.5371385,-0.4985314},
    {-0.9692660,1.8760108,0.0415560},
    {0.0556434,-0.2040259,1.0572252},
};

void rgb2xyY(float *rgb,struct xyY *xyY)
{
    float X,Y,Z;
    float r = rgb[0];
    float g = rgb[1];
    float b = rgb[2];
    printf("rgb: %f %f %f\n",r,g,b);

    if (r > 0.04045f) r = (float)pow((r + 0.055)/1.055, 2.4);
    else r = r / 12.92f;
    if (g > 0.04045) g =(float)pow((g + 0.055f)/1.055f , 2.4f);
    else g = g/12.92f;
    if (b > 0.04045f) b = (float)pow((b + 0.055f)/1.055f , 2.4f);
    else b = b/12.92f;

    r *= 100.0;
    g *= 100.0;
    b *= 100.0;
    printf("rgbl: %f %f %f\n",r,g,b);
    printf("%f %f %f\n",RGB2XYZ[0][0],RGB2XYZ[0][1],RGB2XYZ[0][2]);
    X = r * RGB2XYZ[0][0] + g * RGB2XYZ[0][1] + b * RGB2XYZ[0][2];
    Y = r * RGB2XYZ[1][0] + g * RGB2XYZ[1][1] + b * RGB2XYZ[1][2];
    Z = r * RGB2XYZ[2][0] + g * RGB2XYZ[2][1] + b * RGB2XYZ[2][2];
    printf("X: %f %f %f\n",X,Y,Z);
    xyY->Y = Y;
    float div = X + Y + Z;
    if(div != 0) {
        xyY->x = X /div;
        xyY->y = Y /div;
    } else {
        xyY->x = X;
        xyY->y = Y;
    }
}


void xyY2rgb(struct xyY *xyY,float *rgb)
{
    float X,Y,Z;
    float r,g,b;
    if(xyY->y != 0) {
        float mul = xyY->Y / xyY->y;
        X = xyY->x * mul;
        Y = xyY->Y;
        Z = (1.0 - xyY->x - xyY->y) * mul;
    } else {
        X = 0.0;
        Y = 0.0;
        Z = 0.0;
    }


    X /= 100.0;
    Y /= 100.0;
    Z /= 100.0;

    r = X * XYZ2RGB[0][0] + Y * XYZ2RGB[0][1] + Z * XYZ2RGB[0][2];
    g = X * XYZ2RGB[1][0] + Y * XYZ2RGB[1][1] + Z * XYZ2RGB[1][2];
    b = X * XYZ2RGB[2][0] + Y * XYZ2RGB[2][1] + Z * XYZ2RGB[2][2];
    if (r > 0.0031308f) r = 1.055f*(float)pow(r, (1/2.4f)) - 0.055f;
    else r = 12.92f*r;

    if (g > 0.0031308f) g = 1.055f*(float)pow(g ,(1/2.4f)) - 0.055f;
    else g = 12.92f*g;

    if (b > 0.0031308f) b = 1.055f*(float)pow(b, (1/2.4f)) - 0.055f;
    else b = 12.92f*b;

    rgb[0] = r;
    rgb[1] = g;
    rgb[2] = b;
}

void xyY2data(struct LED_Param *led,struct xyY *xyY,int16_t *data,int bit)
{
    float Dr,Dg,Db;
    //float Ym = 30.0;
    float xr = led->r.x;
    float yr = led->r.y;
    float Yr = led->r.Y;
    printf("R_xyY: %f %f %f\n",xr,yr,Yr);
    float xg = led->g.x;
    float yg = led->g.y;
    float Yg = led->g.Y;
    printf("G_xyY: %f %f %f\n",xg,yg,Yg);
    float xb = led->b.x;
    float yb = led->b.y;
    float Yb = led->b.Y;
    printf("B_xyY: %f %f %f\n",xb,yb,Yb);
    float x = xyY->x;
    float y = xyY->y;
    float Ym = xyY->Y;

    printf("x: %f y: %f Y: %f\n",x,y,xyY->Y);


    Dr = ((yg - yb) * (xb - x)  +  (y - yb) * (xg - xb)) /
         ((yg - yb) * (xb - xr) + (yr - yb) * (xg - xb)) * (yr * Ym) / (y * Yr);

    Dg = ((yb - yr) * (xr - x)  +  (y - yr) * (xb - xr)) /
         ((yb - yr) * (xr - xg) + (yg - yr) * (xb - xr)) * (yg * Ym) / (y * Yg);

    Db = ((yg - yr) * (xr - x) +   (y - yr) * (xg - xr)) /
         ((yg - yr) * (xr - xb) + (yb - yr) * (xg - xr)) * (yb * Ym) / (y * Yb);

    float Y = 1;
    printf("Dr: %f Dg: %f Db: %f\n",Dr,Dg,Db);

    /* if (Dr > 0.0031308f) Dr = 1.055f*(float)pow(Dr, (1/2.4f)) - 0.055f; */
    /* else Dr = 12.92f*Dr; */

    /* if (Dg > 0.0031308f) Dg = 1.055f*(float)pow(Dg ,(1/2.4f)) - 0.055f; */
    /* else Dg = 12.92f*Dg; */

    /* if (Db > 0.0031308f) Db = 1.055f*(float)pow(Db, (1/2.4f)) - 0.055f; */
    /* else Db = 12.92f*Db; */

#define MAX(d0,d1) ((d0 > d1) ? d0 : d1);
    float N = MAX(Dr,Dg);
    N = MAX(N,Db);
    printf("N: %f Y: %f %d\n",N,Y,(1 << bit) - 1);
    N = 1.0;// / N;
    Dr =  Dr / 0.564762 * Y;
    Dg =  Dg / 0.459927 * Y;
    Db =  Db / 0.069321 * Y;

    if (Dr > 0.0031308f) Dr = 1.055f*(float)pow(Dr, (1/2.4f)) - 0.055f;
    else Dr = 12.92f*Dr;

    if (Dg > 0.0031308f) Dg = 1.055f*(float)pow(Dg ,(1/2.4f)) - 0.055f;
    else Dg = 12.92f*Dg;

    if (Db > 0.0031308f) Db = 1.055f*(float)pow(Db, (1/2.4f)) - 0.055f;
    else Db = 12.92f*Db;

    printf("==> Dr: %f Dg: %f Db: %f\n",Dr,Dg,Db);
    data[0] = Dr * 255.0;
    data[1] = Dg * 255.0;
    data[2] = Db * 255.0;
}
